#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>
#include <math.h>
#include <string.h>
#include <unistd.h>


#include <door_detect/in_door.h>

using namespace cv;
using namespace std;



int thresh = 50;
int N = 5; 
const char* wndname = "Square Detection Demo";

void init(ros::NodeHandle& nh) {
  camera_rgb_sub_ = nh.subscribe("/uav6/d435i/color/image_raw", 10, &InDoor::rgbIamgeCallback, this);
  rectangle_rgb_pub_ = nh.advertise<sensor_msgs::Image>("/uav6/rectangle_detect/image_raw", 10);
///uav6/d435i/color/image_raw/ sensor_msgs::CompressedImage

}

// void InDoor::rgbIamgeCallback(const sensor_msgs::ImageConstPtr &msgRGB) {
//    cv_bridge::CvImagePtr cv_ptr;
//   try
//       {
//         // * ROS消息格式转cv::Mat
//         cv_ptr = cv_bridge::toCvCopy(msgRGB,sensor_msgs::image_encodings::BGR8);
//       }
//       catch (cv_bridge::Exception& e)
//       {
//         // log(Error,("video frame0 exception: "+QString(e.what())).toStdString());
//         cout << " video error!! " << endl;
//         return;
//       }
// }
void rgbIamgeCallback(const sensor_msgs::ImageConstPtr &msgRGB) {
  cv_bridge::CvImagePtr cv_ptr;
  // * ROS消息格式转cv::Mat
  cv_ptr = cv_bridge::toCvCopy(msgRGB, sensor_msgs::image_encodings::BGR8);
  img = cv_ptr -> image;
}


// 求向量间夹角的余弦值
double angle (Point pt1, Point pt2, Point pt0) {
  double dx1 = pt1.x - pt0.x;
  double dy1 = pt1.y - pt0.y;
  double dx2 = pt2.x - pt0.x;
  double dy2 = pt2.y - pt0.y;
  return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

void findSquares( const Mat& image, vector<vector<Point> >& squares) {
  squares.clear();
  Mat timg(image);
  medianBlur(image, timg, 9);
  Mat gray0(timg.size(), CV_8U), gray;

  vector<vector<Point> > contours;

  for( int c = 0; c < 3; c++ )
    {
        int ch[] = {c, 0};
        mixChannels(&timg, 1, &gray0, 1, ch, 1);

        // try several threshold levels
        for( int l = 0; l < N; l++ )
        {
            // hack: use Canny instead of zero threshold level.
            // Canny helps to catch squares with gradient shading
            if( l == 0 )
            {
                // apply Canny. Take the upper threshold from slider
                // and set the lower to 0 (which forces edges merging)
                Canny(gray0, gray, 5, thresh, 5);
                // dilate canny output to remove potential
                // holes between edge segments
                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                // apply threshold if l!=0:
                //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
                gray = gray0 >= (l+1)*255/N;
            }

            // find contours and store them all as a list
            findContours(gray, contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            // test each contour
            for( size_t i = 0; i < contours.size(); i++ )
            {
                // approximate contour with accuracy proportional
                // to the contour perimeter
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.02, true);

                // square contours should have 4 vertices after approximation
                // relatively large area (to filter out noisy contours) and be convex.
                // Note: absolute value of an area is used because area may be positive or negative - in accordance with the contour orientation
                if( approx.size() == 4 &&
                    fabs(contourArea(Mat(approx))) > 1000 &&
                    isContourConvex(Mat(approx)) )
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }

                    // if cosines of all angles are small (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence
                    if( maxCosine < 0.3 )
                        squares.push_back(approx);
                }
            }
        }
    }
}

  

//绘制图像中的所有矩形
Mat drawSquares( Mat& image, const vector<vector<Point> >& squares)
{
    for( size_t i = 0; i < squares.size(); i++ )
    {
        const Point* p = &squares[i][0];

        int n = (int)squares[i].size();
        //dont detect the border
        if (p-> x > 3 && p->y > 3)
          polylines(image, &p, &n, 1, true, Scalar(0,255,0), 3, LINE_AA);
    }
    imshow(wndname, image);
    return image;
    
}

void detect_door () {
  //  ===========  获取当前路径 ===========
    char buffer[PATH_MAX];
    if (getcwd(buffer, sizeof(buffer)) != nullptr) {
        std::cout << "当前路径：" << buffer << std::endl;
    } 
    else {
        std::cout << "获取当前路径失败！" << std::endl;
    }
    string path_name = buffer;
    char img_name[] = "/src/door_detect/imgs/2stickies.jpg";
    string path = path_name + img_name;

    namedWindow( wndname, 1);
    // 传入图片
    Mat image = imread(path, 1);


    // Mat image = img;

    vector<vector<Point> > squares;
    if (image.empty()) {
        //  cout << "Couldn't load " << path << endl;
        // continue;
        cout << " image is empty !!! " << endl;
    }
    else {
        findSquares(image, squares);
        Mat image_rec = drawSquares(image, squares);
        // 发布识别得到的结果
        //需要转格式

        sensor_msgs::ImagePtr msg_rec = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_rec).toImageMsg();
        rectangle_rgb_pub_.publish(msg_rec);
    }
}


int main(int argc, char** argv) {
  ros::init(argc, argv, "door_detect");
  ros::NodeHandle nh("~");

  init(nh);

  ros::Duration(1.0).sleep();
  ros::spin();

  char buffer[PATH_MAX];
    if (getcwd(buffer, sizeof(buffer)) != nullptr) {
        std::cout << "当前路径：" << buffer << std::endl;
    } 
    else {
        std::cout << "获取当前路径失败！" << std::endl;
    }
    string path_name = buffer;
    char img_name[] = "/src/door_detect/imgs/2stickies.jpg";
    string path = path_name + img_name;

    namedWindow( wndname, 1);
    // 传入图片
    Mat image = imread(path, 1);


    // Mat image = img;

    vector<vector<Point> > squares;
    if (image.empty()) {
        //  cout << "Couldn't load " << path << endl;
        // continue;
        cout << " image is empty !!! " << endl;
    }
    else {
        findSquares(image, squares);
        Mat image_rec = drawSquares(image, squares);
        // 发布识别得到的结果
        //需要转格式

        sensor_msgs::ImagePtr msg_rec = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_rec).toImageMsg();
        rectangle_rgb_pub_.publish(msg_rec);
    }

    //--------------------------------------------------------------------

  return 0;
}